#include "geometry.h"
#include <algorithm>

namespace
{
   int x_compare(const void * a1, const void * b1)
   {
      point a = *(point*)a1;
      point b = *(point*)b1;

      if (a.x == b.x)
      {
         return 0;
      }

      if (a.x < b.x)
      {
         return -1;
      }

      return 1;
   }

   int y_compare(const void * a1, const void * b1)
   {
      point a = *(point*)a1;
      point b = *(point*)b1;

      if (a.y == b.y)
      {
         return 0;
      }

      if (a.y < b.y)
      {
         return -1;
      }

      return 1;
   }
}
kd2tree::kd2tree(const std::vector<point> & points)
{
   nodes.resize(points.size() * 4 + 1);

   point * arr = new point[points.size()];
   int n = points.size();
   for (int i = 0;  i < n; i++)
   {
      arr[i] = points[i];
   }

   make_tree(arr, n, 1, true);
   delete[] arr;
}

void kd2tree::make_tree(point* arr, int arr_size, int pos, bool compare)
{
   if (arr_size == 0)
   {
      return;
   }

   if (arr_size == 1)
   {
      nodes[pos] = node(arr[0], compare);
      return;
   }

   if (compare)
   {
      std::qsort(arr, arr_size, sizeof(point), x_compare);
   } else
   {
      std::qsort(arr, arr_size, sizeof(point), y_compare);
   }

   nodes[pos] = node(arr[arr_size / 2], compare);
   int n1 = arr_size / 2;
   point * arr1 = new point[n1];
   for (int i = 0; i < n1; i++)
   {
      arr1[i] = arr[i];
   }
   make_tree(arr1, n1, 2 * pos, !compare);
   delete[] arr1;

   int n2 = arr_size - n1 - 1;
   point * arr2 = new point[n2];
   for (int i = n1 + 1; i < arr_size; i++)
   {
      arr2[i - n1 - 1] = arr[i];
   }
   make_tree(arr2, n2, 2 * pos + 1, !compare);
   delete[] arr2;
}

std::vector<point> kd2tree::find_points(point ll, point rh)
{
   return get_points(ll, rh, 1);
}

std::vector<point> kd2tree::get_points(point ll, point rh, int pos)
{
    std::vector<point> ans;
   if (!nodes[pos].exist)
   {
      return ans;
   }
   if (nodes[pos].p.x >= ll.x  && nodes[pos].p.y >= ll.y && nodes[pos].p.x <= rh.x && nodes[pos].p.y <= rh.y)
   {
      ans.push_back(nodes[pos].p);
   }

   if (ll.x <= nodes[pos].p.x && nodes[pos].orientation ||
       !nodes[pos].orientation && ll.y <= nodes[pos].p.y)
   {
      std::vector<point> temp = get_points(ll, rh, pos * 2);
      int n = temp.size();
      for (int i = 0; i < n; i++)
      {
        ans.push_back(temp[i]);  
      }
   }

   if (rh.x >= nodes[pos].p.x && nodes[pos].orientation ||
        !nodes[pos].orientation && rh.y >= nodes[pos].p.y)
   {
      std::vector<point> temp = get_points(ll, rh, pos * 2 + 1);
      int n = temp.size();
      for (int i = 0; i < n; i++)
      {
         ans.push_back(temp[i]);
      }
   }
   return ans;
}